#!/usr/bin/env python
import roslib; roslib.load_manifest('block_build_manager')
import rospy
import sys

from block_build_msgs.msg import *
from std_msgs.msg import *

# Sends a string message to slu_command_parser in BuildManager.py to pick up a block and place it at a specified (x,y) location
SLU_TOPIC_NAME = "/blocknlp/nl_output"
SLU_COMMAND_PARSER_STATUS_TOPIC_NAME = "/blocknlp/slu_command_parser_status"

# Sends a command to BuildManager to pick up a block by its specified id number
def pickup(x):
  msg = SLUCommand()
  msg.command = "pickup(" + str(x) + ")"
  rospy.loginfo(msg.command)
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to place a held block at a specified (x,y) position
def place_block(x,y):
  msg = SLUCommand()
  msg.command = "putdown(0,[" + str(x) + "," + str(y) +"])"
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to pick up a block by its specified id number and to place the block at a specified (x,y) position
def pick_place(id,x,y):
  msg = SLUCommand()
  msg.command = "(pickup(" + str(id) + "), putdown(0,[" + str(x) + "," + str(y) +"]),)"
  rospy.loginfo(msg.command)
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to drop a held block
def drop_block():
  msg = SLUCommand()
  msg.command = "dropblock()"
  rospy.loginfo(msg.command)
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to stack a block on another block with the specified id number
def stackon(x):
  msg = SLUCommand()
  msg.command = "stackon(" + str(x) + ")"
  rospy.loginfo(msg.command)
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to return a held block back to its toolbox position
def return_block_to_home_position():
  msg = SLUCommand()
  msg.command = "returnBlockToHomePosition()"
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Sends a command to BuildManager to reset the toolbox
def resetToolbox():
  msg = SLUCommand()
  msg.command = "resetToolBox()"
  rospy.loginfo(msg.command)
  pub.publish(msg)
  rospy.wait_for_message(SLU_COMMAND_PARSER_STATUS_TOPIC_NAME,Empty,20.0)

# Set up a cross configuration and spreads the blocks apart
def crossTest():
  pick_place(0,0.575, 0.075)
  pick_place(1,0.575,-0.075)
  pick_place(2,0.425, 0.075)
  pick_place(3,0.425,-0.075)

  pick_place(0,0.65, 0.15)
  pick_place(1,0.65,-0.15)
  pick_place(2,0.35, 0.15)
  pick_place(3,0.35,-0.15)


# Lines up three blocks and jumps the blocks twice
def jumpTest():
  pick_place(0,0.5,0.075)
  pick_place(1,0.5,0)
  pick_place(2,0.5,-0.15)

  pick_place(0,0.5,-0.075)
  pickup(1)
  return_block_to_home_position()
  
  pick_place(2,0.5,0)
  pickup(0)
  return_block_to_home_position()

# Aligns three blocks into a straight line
def alignTest():
  pick_place(3,0.575, 0.23)
  pick_place(1,0.6,-0.17)
  pick_place(2,0.425, 0.05)

  pick_place(3,0.5,0.15)
  pick_place(2,0.5,0)
  pick_place(1,0.5,-0.15)
  
if __name__ == '__main__':

  pub = rospy.Publisher(SLU_TOPIC_NAME,SLUCommand)
  rospy.init_node('robustness_test_node')
  for i in range(2):
    pick_place(0, 0.5, -0.2)
    pick_place(4, 0.5, 0.2)
    resetToolbox()
